/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it
under the terms of the GNU Lesser General Public License as published
by the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
License for more details.

You should have received a copy of the GNU Lesser General Public
License along with GIMnet. (See COPYING.LESSER) If not, see
<http://www.gnu.org/licenses/>.


**/
/**
 *
 * $Id: JointGroupCtrlClientExample.cpp,v 1.4 2009-08-27 14:12:35 morsko Exp $
 *
 * \file
 * \brief This is an example for using the JointGroupCtrl interface.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *edited by Tilman Pfeiffer to fit the FSR 2011 Group 5 needs
 */
#include "interface_header.hpp"
#include "JointGroupCtrlClient.hpp"
#include <curses.h>
#include <math.h>
//*****************************************************************************
#define GIMI_CLIENT_API 20000
#define TORAD 0.017453292
#define TODEG 57.29577951
#include "gimi.h"
//*****************************************************************************
// Objects
static bool run = true;
static gimi::GIMI g;
static MaCI::JointGroupCtrl::CJointGroupCtrlClient jgcc(&g, 0);

using namespace MarsNamespace;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//static int verbose = 1;
//*****************************************************************************
//*****************************************************************************

static void DoJointGroupCtrl()
{
  // Initialize Curses (After this point, don't use printf family calls!)
  initscr();
  cbreak();
  //noecho();
  nonl();
  intrflush(stdscr, FALSE);
  keypad(stdscr, TRUE);
  scrollok(stdscr, TRUE);
  //nodelay(stdscr, TRUE);
  //halfdelay(10);

  // After this point, we don't want dPrint to print to Screen, so we disable it.
  debugSetPrintLevel(2);

  unsigned int speedSeq = 0;
  unsigned int positionSeq = 0;
  //unsigned int forceSeq = 0;
  clear();
  printw("Welcome to IO test program.\n\n");
  using namespace MaCI::JointGroupCtrl;
  do {
    int value;
    printw("\nSelect JointGroupCtrl Query:\n");
    printw(
    "KEY_UP = x\n"
	   "KEY_DOWN = -x\n"
	   "KEY_LEFT = y\n"
	   "KEY_RIGHT= -y\n"
	   " 8 = vertical up\n"
	   " / = vertical all up\n"
       " 2 = vertical down\n"
       " 5 = vertical all down\n"
	   " 7 = gripper close\n"
       " 9 = gripper open\n"
       " 4 = wrist left\n"
	   " 6 = wrist right\n"
       " 1 = direct angle -E\n"
       " 3 = direct angle E\n"
       " 0 = direct angle -S\n"
       " , = direct angle S\n"
        "s = reset micro\n"
//           "S = Set speeds of many joints\n"
//	   "a = Get joint Speed\n"
//           "A = Get speeds of multiple Joints\n"
	   "p = Set joint Position\n"
           "P = Set position of many joints\n"
	   "o = Get joint Position\n"
           "O = Get positions of multiple joints\n"
//	   "f = Set joint Force\n"
//           "F = Set forces of many joints\n"
//	   "d = Get joint Force\n"
//           "D = Get forces of multiple joints\n"
	   "I = Get joint Info\n"
	   "n = Get number of joints\n"
	   "i = Get Jointgroup information\n"
           "j = Get jointgroup status\n"
	   "q = QUIT\n");
    printw("?\n");

    value = getch();
    clear(); // After input, clear the screen. Not after processing!
    //MaCI::JointGroupCtrl::EJointGroupStatus currentstatus;
    //jgcc.GetJointGroupStatus(currentstatus,1000);
    ownSleep_ms(100);
    switch(value){

 //get current position and add an increment to it to move step by step
    case KEY_UP:{//x
        float pos;
        jgcc.GetPosition(0,pos,1000);
        jgcc.SetPosition(20+pos,0);
        break;
    }
    case KEY_DOWN:{//-x
	        float pos;
            jgcc.GetPosition(0,pos,1000);
            jgcc.SetPosition(-20+pos,0);
        break;
    }
    case KEY_LEFT:{//-y
	        float pos;
            jgcc.GetPosition(1,pos,1000);
            jgcc.SetPosition(20+pos,1);
        break;
    }
    case KEY_RIGHT:{//y
	        float pos;
            jgcc.GetPosition(1,pos,1000);
            jgcc.SetPosition(-20+pos,1);;
        break;
    }
    case '0':{//shoulder left
	        float pos;
            jgcc.GetPosition(4,pos,1000);
            jgcc.SetPosition(10+pos,4);
        break;
    }
    case ',':{//shoulder right
	        float pos;
            jgcc.GetPosition(4,pos,1000);
            jgcc.SetPosition(-10+pos,4);
        break;
    }
    case '1':{//elbow left
	        float pos;
            jgcc.GetPosition(5,pos,1000);
            jgcc.SetPosition(10+pos,5);
        break;
    }
    case '3':{//elbow right
	        float pos;
            jgcc.GetPosition(5,pos,1000);
            jgcc.SetPosition(-10+pos,5);
        break;
    }
    case '8':{//linerActuatorUP
	        float pos;
            jgcc.GetPosition(6,pos,1000);
            jgcc.SetPosition(-25+pos,6);
        break;
    }

    case '2':{//linearActuatorDown
	        float pos;
            jgcc.GetPosition(6,pos,1000);
            jgcc.SetPosition(25+pos,6);
        break;
    }
    case '4':{//wrist left
            float pos;
            jgcc.GetPosition(7,pos,1000);
            jgcc.SetPosition(10+pos,7);
        break;
    }
    case '6':{//wrist right
	        float pos;
            jgcc.GetPosition(7,pos,1000);
            jgcc.SetPosition(-10+pos,7);
        break;
    }
    case '7':{//gripper open
            float pos;
            jgcc.GetPosition(8,pos,1000);
            jgcc.SetPosition(-10+pos,8);
        break;
    }
    case '9':{//gripper close
	        float pos;
            jgcc.GetPosition(8,pos,1000);
            jgcc.SetPosition(10+pos,8);
        break;
    }

    //----------------------end of step by step input.

    case '5':{//linear actuator to maximum extension
	jgcc.SetPosition(140,6);
    break;
    }
    case '/':{//linear actuator to minimum extension
	jgcc.SetPosition(0,6);
    break;
    }


   case 's':{
      //int r;
      //float speed;
      //int jointNumber;
      //printw("Enter speed and joint Number: ");
      //r = scanw("%f %d",&speed, &jointNumber);
      //if (r >= 2) {
	jgcc.SetSpeed(0,0);
      //}
      break;
    }/*
    case 'S':{
      float speed;
      int jointNumber,r;
      printw("Enter speed and joint Number (Leave empty when finished): ");
      r = scanw("%f %d",&speed, &jointNumber);

      TJointSpeedArray array;
      while(r >=2){

	array.push_back(TJointSpeed(speed,jointNumber));
        printw("Enter speed and joint Number (Leave empty when finished): ");
        r = scanw("%f %d",&speed, &jointNumber);
      }
      jgcc.SetGroupSpeed(array);
      break;
    }*/
    case 'p':{
      float position;
      int jointNumber,r;
      printw("Enter position and joint Number: ");
      r = scanw("%f %d",&position, &jointNumber);
      if (r >= 2) {
	jgcc.SetPosition(position,jointNumber);
      }

      break;
    }
    case 'P':{
      float position;
      int jointNumber,r;
      printw("Enter position and joint Number (Leave empty when finished): ");
      r = scanw("%f %d",&position, &jointNumber);

      TJointPositionArray array;
      while(r >=2){

        array.push_back(TJointPosition(position,jointNumber));
        printw("Enter position and joint Number (Leave empty when finished): ");
        r = scanw("%f %d",&position, &jointNumber);
      }
      jgcc.SetGroupPosition(array);
      break;
    }
/*    case 'f':{
      float force;
      int jointNumber,r;
      printw("Enter force and joint Number: ");
      r = scanw("%f %d",&force, &jointNumber);
      if (r >= 2) {
	jgcc.SetForce(force,jointNumber);
      }

      break;
    }
    case 'F':{
      float force;
      int jointNumber,r;
      printw("Enter force and joint Number (Leave empty when finished): ");
      r = scanw("%f %d",&force, &jointNumber);

      TJointForceArray array;
      while(r >=2){

	array.push_back(TJointForce(force,jointNumber));
        printw("Enter force and joint Number (Leave empty when finished): ");
        r = scanw("%f %d",&force, &jointNumber);
      }
      jgcc.SetGroupForce(array);
      break;

    }*/
    case 'I':{
      int jointNumber, r;
      bool ret;
      MaCI::JointGroupCtrl::TJointInfo info("fuubar", 0, MaCI::JointGroupCtrl::KJointControlAngular);
      printw("Enter joint Number: ");
      r = scanw("%d", &jointNumber);
      if (r >= 1) {
        ret = jgcc.GetJointInformation(info,jointNumber,1000);
      }
      if (!ret){
		printw("FAILED!\n");
		break;
      }
	  printw("%d. name: %s, number %d",jointNumber,
		 info.jointName.c_str(),
		 info.jointNumber);
	  if(info.jointControlType == MaCI::JointGroupCtrl::KJointControlAngular){
	    printw(", takes control commands with angular units,");
	  }else if(info.jointControlType == MaCI::JointGroupCtrl::KJointControlLinear){
	    printw(", takes control commands with length units,");
	  }

	  if(info.canControlSpeed){
	    printw(" Can be controlled by speed commands, maxSpeed %f",info.maxSpeed);
	  }
	  if(info.canControlPosition){
	    printw(" Can be controlled by position commands, minPosition %f, maxPosition %f",
		   info.minPosition,
		   info.maxPosition);
	  }
          if(info.canControlForce){
            printw(" Can be controlled by force commands, minForce %f, maxForce %f",
                   info.minForce,
                   info.maxForce);
          }
	  printw("\n");

      break;
    }
    case 'i':{
      MaCI::JointGroupCtrl::TJointGroupInformation info;
      printw("Getting information...");
      if(jgcc.GetJointGroupInformation(info,1000)){
	printw("SUCCESS!\n");
	int numberOfJoints = info.size();
	for(int i = 0; i< numberOfJoints; i++){
	  printw("%d. name: %s, number %d",i,
		 info[i].jointName.c_str(),
		 info[i].jointNumber);
	  if(info[i].jointControlType == MaCI::JointGroupCtrl::KJointControlAngular){
	    printw(", It is an angular joint");
	  }else if(info[i].jointControlType == MaCI::JointGroupCtrl::KJointControlLinear){
	    printw(", It is a linear joitn");
	  }

	  if(info[i].canControlSpeed){
	    printw(", Can be controlled by speed commands, maxSpeed %f",info[i].maxSpeed);
	  }
	  if(info[i].canControlPosition){
	    printw(", Can be controlled by position commands, minPosition %f, maxPosition %f",
		   info[i].minPosition,
		   info[i].maxPosition);
	  }
          if(info[i].canControlForce){
            printw(", Can be controlled by force commands, minForce %f, maxForce %f",
                   info[i].minForce,
                   info[i].maxForce);
          }
	  printw("\n");

	}

      }else{
	printw("FAILED!\n");
      }

      break;
    }
    case 'a':{
      int jointNumber,r;
      float speed;
      unsigned int timeout = 1000;
      printw("Enter joint Number: ");
      r = scanw("%d", &jointNumber);
      if (r >= 1) {
	printw("Getting speed (timeout %u ms)...",timeout);
	if(jgcc.GetSpeed(jointNumber,speed,timeout)){
	  printw("SUCCESS!\n");
	  printw("Joint n.%d speed is %f",
		 jointNumber,
		 speed);
	}else{
	printw("FAILED!\n");
	}
      }
      break;
    }
    case 'A':{

      unsigned int timeout = 1000;
      printw("Getting group speeds (timeout %u ms)... ",timeout);
      TJointSpeedArray speedArray;

      if(jgcc.GetGroupSpeeds(speedArray,&speedSeq,timeout)){
        printw("SUCCESS!\n");
        for(EACH_IN_i(speedArray)){
          printw("Joint n.%d speed is %f\n",
		 i->jointNumber,
		 i->speed);
        }
      }else{
	printw("FAILED!\n");
      }
      break;


    }
    case 'o':{
      int jointNumber,r;
      float position;
      unsigned int timeout = 1000;
      printw("Enter joint Number: ");
      r = scanw("%d", &jointNumber);
      if (r >= 1) {
	printw("Getting position (timeout %u ms)...",timeout);
	if(jgcc.GetPosition(jointNumber,position,timeout)){
	  printw("SUCCESS!\n");
	  printw("Joint n.%d position is %f",
		 jointNumber,
		 position);
	}else{
	printw("FAILED!\n");
	}
      }
      break;
    }
    case 'O':{

      unsigned int timeout = 1000;
      printw("Getting group positions (timeout %u ms)... ",timeout);
      TJointPositionArray positionArray;
      if(jgcc.GetGroupPositions(positionArray,&positionSeq,timeout)){
        printw("SUCCESS!\n");
        for(EACH_IN_i(positionArray)){
          printw("Joint n.%d position is %f\n",
		 i->jointNumber,
		 i->position);
        }
      }else{
	printw("FAILED!\n");
      }
      break;

    }
  /*  case 'd':{
      int jointNumber,r;
      float force;
      unsigned int timeout = 1000;
      printw("Enter joint Number: ");
      r = scanw("%d", &jointNumber);
      if (r >= 1) {
	printw("Getting force (timeout %u ms)...",timeout);
	if(jgcc.GetForce(jointNumber,force,timeout)){
	  printw("SUCCESS!\n");
	  printw("Joint n.%d force is %f",
		 jointNumber,
		 force);
	}else{
          printw("FAILED!\n");
	}
      }
      break;
    }
    case 'D':{

      unsigned int timeout = 1000;
      printw("Getting group forces (timeout %u ms)... ",timeout);
      TJointForceArray forceArray;

      if(jgcc.GetGroupForces(forceArray,&forceSeq,timeout)){
        printw("SUCCESS!\n");
        for(EACH_IN_i(forceArray)){
          printw("Joint n.%d force is %f\n",
		 i->jointNumber,
		 i->force);
        }
      }else{
	printw("FAILED!\n");
      }
      break;


    }*/
    case 'n':{
      int j;
      printw("Getting joint Number... ");
	  j = jgcc.GetJointNumber();
	  printw("%d joints found \n", j);
      break;
    }

    case 'j':{
      MaCI::JointGroupCtrl::EJointGroupStatus status;
      printw("Getting group status...");
      if(jgcc.GetJointGroupStatus(status,1000)){
        printw("SUCCESS!\n");
	  printw("Joint group status is %d (open = %d, closed = %d, unimplemented = %d",
		 status,
                 MaCI::JointGroupCtrl::KJointGroupStatusOpen,
                 MaCI::JointGroupCtrl::KJointGroupStatusClosed,
                 MaCI::JointGroupCtrl::KJointGroupStatusUnimplemented);

      }else{
	printw("FAILED!\n");
      }

      break;

    }
    case 'q':{

      run = false;
      printw("\nBYE BYE!.\n\n");

      break;
    }
    default:{
      clear();
      printw("\nUnknown command.\n\n");
      continue;
      break;
    }
    }

 } while(run);

  // Close curses.
  endwin();

}


//*****************************************************************************
void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tJointGroupCtrlClientExample [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-s name]\t\t\tUse 'name' as SpeedCtrl target\n");
  fprintf(stderr,"  \t\t\t\tOR\n");
  fprintf(stderr,"  [-S name]\t\t\tSpecify the data source by MaCISL\n\n");
  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************
MarsManipulator::MarsManipulator( std::string APip,  int port, std::string groupname, std::string sourcename){

    std::string gimnetAP = APip;
    int gimnetAPPort = port;
    std::string gimnetName = groupname;
    std::string gimnetSourceName = sourcename;
//int main(int argc, char *argv[]) {
  // GIMnet parameters
  /*std::string gimnetAP = "localhost";
  int gimnetAPPort = 40002;
  std::string gimnetName = "mars";
  std::string gimnetSourceName = "Control";*/
  std::string datasourceMaCISL;
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(2);
  debugSetLogFilename("JointGroupCtrl_Client_example.log");
  debugSetLogLevel(6);
  // Greet
  fprintf(stderr,
	  "\nJointGroupCtrlClientExample - MaCI JointGroupCtrl Interface Example - v0.0\n");

  // Parse parameters
  //bool parse = true;
  /*
  while(parse) {
    int c;
    c = getopt(argc, argv, "s:S:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 's':
      gimnetSourceName = optarg;
      dPrint(3,"Using GIMnet source name '%s'", optarg);
      break;

    case 'S':
      datasourceMaCISL = optarg;

      dPrint(3,"Using DataSource MaCISL '%s'", optarg);
      break;

    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;

    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;

    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;

    case -1:
      parse = false;
      break;
    }
  }

*/
  MaCI::EMaCIError e;

  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(),
	   gimnetAPPort,
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());

  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(),
	   gimnetAPPort,
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());



    // Assign default target.
    if (gimnetSourceName.size()) {
      jgcc.SetDefaultTarget(gimnetSourceName);

    } else {
      // GIMI is Open(), so we can execute this.
      MaCI::MaCICtrl::SMaCISL sl(datasourceMaCISL);
      if (sl.IsValidFQMaCISL()) {
        jgcc.SetDefaultTarget(sl, 10000);
      }

    }
    // Attempt to Open Client
    if ( (e = jgcc.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open JointGroupCtrlClient instance! (%s)",
	     GetErrorStr(e).c_str());

    } else {

      //DoJointGroupCtrl();
      //jgcc.Close();
    }
  }


}



MarsManipulator::~MarsManipulator(){
    jgcc.Close();
}


//

MarsManipulator::ECommandResult MarsManipulator::changeSpeed(int speed){
  ECommandResult result= UNKNOWN;
  if(jgcc.SetSpeed(speed,1)) result = SUCCESS;
  else result = FAILURE;
  return result;
  }

	// Picking up objects
MarsManipulator::ECommandResult MarsManipulator::pickObject(double atx,double aty){
        using namespace MaCI::JointGroupCtrl;
        ECommandResult result= UNKNOWN;
        TJointPositionArray array;
         array.push_back(TJointPosition(atx,0));
        array.push_back(TJointPosition(aty,1));
        jgcc.SetGroupPosition(array);
        result = isCommandExecuted();
        return result;
    }

MarsManipulator::ECommandResult MarsManipulator::movewithconstCto(double atx,double aty){
        using namespace MaCI::JointGroupCtrl;
        ECommandResult result= UNKNOWN;
        TJointPositionArray array;
         array.push_back(TJointPosition(atx,0));
        array.push_back(TJointPosition(aty,1));
        array.push_back(TJointPosition(0,3));
        jgcc.SetGroupPosition(array);
        result = isCommandExecuted();
        return result;
    }

	// Picking up objects from manipulator camera click
MarsManipulator::ECommandResult MarsManipulator::movetoPosfromI(float clickedy, float clickedx){
        using namespace MaCI::JointGroupCtrl;
        ECommandResult result= UNKNOWN;
                float gripperx= (clickedy - 250 )*(-1);
                float grippery= (clickedx - 400)*(-1);
                gripperx*=250.0/480.0;
                grippery*=330.0/640.0;
                float currentC, currentX, currentY;
                jgcc.GetPosition(3,currentC,1000);
                jgcc.GetPosition(0,currentX,1000);
                jgcc.GetPosition(1,currentY,1000);
                float cosinusC =cos(currentC*TORAD);
                float sinusC =sin(currentC*TORAD);
                float clickedXinManCoords =cosinusC*gripperx-sinusC*grippery + currentX;
                float clickedYinManCoords =sinusC*gripperx+cosinusC*grippery + currentY;
                dPrint(1,"convert to manipulator coordinates: (%f, %f) \n", clickedXinManCoords,clickedYinManCoords);
                MaCI::JointGroupCtrl::TJointPositionArray array;
                array.push_back(MaCI::JointGroupCtrl::TJointPosition(clickedXinManCoords,0));
                array.push_back(MaCI::JointGroupCtrl::TJointPosition(clickedYinManCoords,1));
                jgcc.SetGroupPosition(array);
                result = isCommandExecuted();
        return result;
    }

MarsManipulator::ECommandResult MarsManipulator::toBottom(double scale){
    if (scale< 0) scale = 0;
    else if (scale > 1) scale = 1;
    
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition((int)(140 * scale), 6);
    result = isCommandExecuted();
    return result;
    }
MarsManipulator::ECommandResult MarsManipulator::toTop(){
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition(0, 6);
    result = isCommandExecuted();
    return result;
    }

MarsManipulator::ECommandResult MarsManipulator::grab(){
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition(140, 8);
    result = isCommandExecuted();
    return result;
    }

MarsManipulator::ECommandResult MarsManipulator::release(){
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition(120, 8);
    result = isCommandExecuted();
    return result;
    }

MarsManipulator::ECommandResult MarsManipulator::grabObject(){
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition(120,8);
    isCommandExecuted();
    jgcc.SetPosition(60, 2);
     if ( (result = isCommandExecuted()) == SUCCESS ){
        jgcc.SetPosition(135,8);
        result = isCommandExecuted();
     }
     return result;
}


MarsManipulator::ECommandResult MarsManipulator::extendArm(){
    using namespace MaCI::JointGroupCtrl;
    ECommandResult result= UNKNOWN;
    TJointPositionArray array;
     array.push_back(TJointPosition(0,4));
     array.push_back(TJointPosition(0,5));
     array.push_back(TJointPosition(0,7));
    jgcc.SetGroupPosition(array);
    result = isCommandExecuted();
    return result;
}
MarsManipulator::ECommandResult MarsManipulator::retractArm(int pocket){
    using namespace MaCI::JointGroupCtrl;
    ECommandResult result= UNKNOWN;
    float sign= 0;
    sign=( pocket ? -1 : 1);
    //close gripper first to avoid collisions
    jgcc.SetPosition(135,8);
    result = isCommandExecuted();

    //move to position
    TJointPositionArray array;
     array.push_back(TJointPosition(sign*-90,4));
     array.push_back(TJointPosition(sign*90,5));
     array.push_back(TJointPosition(sign*90,7));
     jgcc.SetGroupPosition(array);
     result = isCommandExecuted();

    return result;
}

// Storing and delivering objects
MarsManipulator::ECommandResult MarsManipulator::deliverFromManipulator(){
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition(120,8);
    result=isCommandExecuted();

    return result;
}

MarsManipulator::ECommandResult MarsManipulator::deliverFromPocket(int pocket){
    return UNKNOWN;
}

MarsManipulator::ECommandResult MarsManipulator::storeObjectToPocket(int pocket){
    using namespace MaCI::JointGroupCtrl;
    ECommandResult result= UNKNOWN;
    float sign= 0;
    sign=( pocket ? -1 : 1);

     jgcc.SetPosition(0,6);
     isCommandExecuted();
     TJointPositionArray array0,array1;
     array0.push_back(TJointPosition(sign*-90,4));
     array0.push_back(TJointPosition(sign*90,5));
     array0.push_back(TJointPosition(sign*80,7));
     jgcc.SetGroupPosition(array0);
     isCommandExecuted();
     TJointPositionArray arrayim;
     arrayim.push_back(TJointPosition(0,4));
     arrayim.push_back(TJointPosition(sign*70,5));
     arrayim.push_back(TJointPosition(sign*70,7));
     jgcc.SetGroupPosition(arrayim);
     isCommandExecuted();
     if(pocket == 0){
     array1.push_back(TJointPosition(sign*90,4));
     array1.push_back(TJointPosition(sign*15,5));
     array1.push_back(TJointPosition(sign*50,7));
     jgcc.SetGroupPosition(array1);
     } else if (pocket == 1) {
     array1.push_back(TJointPosition(sign*80,4));
     array1.push_back(TJointPosition(sign*45,5));
     array1.push_back(TJointPosition(sign*37,7));
     jgcc.SetGroupPosition(array1);
     }
    if(isCommandExecuted() == SUCCESS){
        jgcc.SetPosition(120, 8);
        result = isCommandExecuted();
    } else result = FAILURE;
        TJointPositionArray array2;
        array2.push_back(TJointPosition(0,4));
        array2.push_back(TJointPosition(sign*70,5));
        array2.push_back(TJointPosition(sign*70,7));
        jgcc.SetGroupPosition(array2);
        isCommandExecuted();
	jgcc.SetPosition(140,8);
	isCommandExecuted();
        TJointPositionArray array;
        array.push_back(TJointPosition(sign*-90,4));
        array.push_back(TJointPosition(sign*90,5));
        array.push_back(TJointPosition(sign*90,7));
        jgcc.SetGroupPosition(array);
        isCommandExecuted();
    return result;
}

MaCI::JointGroupCtrl::TJointPositionArray MarsManipulator::getManipulatorPosition(){
          unsigned int timeout = 1000;
      printw("Getting group positions (timeout %u ms)... ",timeout);
      MaCI::JointGroupCtrl::TJointPositionArray positionArray;
      unsigned int positionSeq = 0;
      if(jgcc.GetGroupPositions(positionArray,&positionSeq,timeout)){
        printw("SUCCESS!\n");
        //for(EACH_IN_i(positionArray)){
         // printw("Joint n.%d position is %f\n",
		 //i->jointNumber,
		 //i->position);
        }
      else{
	printw("FAILED!\n");
      }
      return positionArray;
    }

//manipulator control
MarsManipulator::ECommandResult MarsManipulator::closeManipulator(){return UNKNOWN;}
MarsManipulator::ECommandResult MarsManipulator::openManipulator(){
	DoJointGroupCtrl();
	return UNKNOWN;
}
//*****************************************************************************
MarsManipulator::ECommandResult MarsManipulator::isCommandExecuted(){
        ECommandResult result = UNKNOWN;
        //check if done
        MaCI::JointGroupCtrl::EJointGroupStatus status;
        int statustimeout =10000;
        if (jgcc.GetJointGroupStatus(status,statustimeout)) {
            switch (status) {
            case MaCI::JointGroupCtrl::KJointGroupStatusClosed:
            case MaCI::JointGroupCtrl::KJointGroupStatusOpen: {
                fprintf(stderr,"Command successfully executed \n");
                result = SUCCESS;
                break;
                }
            case MaCI::JointGroupCtrl::KJointGroupStatusReset:{
                fprintf(stderr,"Command exceeded limits \n");
                result = FAILURE;
                break;
                }
            case MaCI::JointGroupCtrl::KJointGroupStatusUnimplemented:{
                fprintf(stderr,"UNKOWN \n");
             break;}
            }
        } else {result = FAILURE;
                fprintf(stderr,"Command not sent \n");
        }
    return result;
}
//*****************************************************************************
//joint movement
//*****************************************************************************
MarsManipulator::ECommandResult MarsManipulator::moveShoulder(int angle){
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition(angle, 4);
    result = isCommandExecuted();
    return result;
}
MarsManipulator::ECommandResult MarsManipulator::moveElbow(int angle){
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition(angle, 5);
    result = isCommandExecuted();
    return result;
}
MarsManipulator::ECommandResult MarsManipulator::moveWrist(int angle){
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition(angle, 7);
    result = isCommandExecuted();
    return result;
}
MarsManipulator::ECommandResult MarsManipulator::moveGripper(int angle){
    ECommandResult result= UNKNOWN;
    jgcc.SetPosition(angle, 8);
    result = isCommandExecuted();
    return result;
}
//*****************************************************************************
//incremental
MarsManipulator::ECommandResult MarsManipulator::moveStepX(){
    ECommandResult result= UNKNOWN;
     float pos;
    jgcc.GetPosition(0,pos,1000);
    jgcc.SetPosition(20+pos,0);
    result = isCommandExecuted();
    return result;
}
MarsManipulator::ECommandResult MarsManipulator::movenegStepX(){
    ECommandResult result= UNKNOWN;
     float pos;
    jgcc.GetPosition(0,pos,1000);
    jgcc.SetPosition(-20+pos,0);
    result = isCommandExecuted();
    return result;
}
MarsManipulator::ECommandResult MarsManipulator::moveStepY(){
    ECommandResult result= UNKNOWN;
     float pos;
    jgcc.GetPosition(1,pos,1000);
    jgcc.SetPosition(20+pos,1);
    result = isCommandExecuted();
    return result;
}
MarsManipulator::ECommandResult MarsManipulator::movenegStepY(){
    ECommandResult result= UNKNOWN;
     float pos;
    jgcc.GetPosition(1,pos,1000);
    jgcc.SetPosition(-20+pos,1);
    result = isCommandExecuted();
    return result;
}
//*****************************************************************************
MarsManipulator::ECommandResult MarsManipulator::resetMicro(){
    jgcc.SetSpeed(1,0);
    ownSleep_ms(500);

    return SUCCESS;
}
//*****************************************************************************
MarsManipulator::ECommandResult MarsManipulator::specifyCorners(std::vector<float> corners){
    using namespace MaCI::JointGroupCtrl;
    TJointForceArray array;
	array.push_back(TJointForce(corners[0],0));
	array.push_back(TJointForce(corners[1],0));
	array.push_back(TJointForce(corners[2],0));
	array.push_back(TJointForce(corners[3],0));
	array.push_back(TJointForce(corners[4],0));
	array.push_back(TJointForce(corners[5],0));
	array.push_back(TJointForce(corners[6],0));
	array.push_back(TJointForce(corners[7],0));
      jgcc.SetGroupForce(array);
    return SUCCESS;
}
//*****************************************************************************
//*****************************************************************************
